﻿/*
 * BursaWolfGeographicTransform
 * 
 * initial implementation : 20.11.2008
 *
 * Copyright (c) Adam Sida, asida@centrum.cz.
 * All rights reserved.
 * 
 * for license see Petra.Gis.License.txt
 */
using System;
using System.Collections.Generic;
using Petra.Gis.Geometry.Support;


namespace Petra.Gis.ReferenceSystemServices.Transformations
{

  /// <summary>
  /// Bursa-Wolf transform is 7 parameter matrix transform defined as
  /// subset of Helmert transform with specific rotation parameter orientation
	/// Parameters for a geographic transformation between datums (typically to or from WGS84).
  /// The Bursa Wolf parameters should be applied 
	/// to geocentric coordinates, where the X axis points towards the Greenwich Prime Meridian, the Y axis
	/// points East, and the Z axis points North.
  /// For simple use this is class that provides 3 steps of transform in one: Geographics to geocentric coordinate transform,
  /// Geocentric to Geocentric using bursa-wolf and geocentric to geographics of target spheroid
	/// </summary>
	/// <remarks>
	/// <para>These parameters can be used to approximate a transformation from the horizontal datum to the
	/// WGS84 datum using a Bursa Wolf transformation. However, it must be remembered that this transformation
	/// is only an approximation. For a given horizontal datum, different Bursa Wolf transformations can be
	/// used to minimize the errors over different regions.</para>
	/// <para>If the DATUM clause contains a TOWGS84 clause, then this should be its “preferred” transformation,
	/// which will often be the transformation which gives a broad approximation over the whole area of interest
	/// (e.g. the area of interest in the containing geographic coordinate system).</para>
	/// <para>Sometimes, only the first three or six parameters are defined. In this case the remaining
	/// parameters must be zero. If only three parameters are defined, then they can still be plugged into the
	/// Bursa Wolf formulas, or you can take a short cut. The Bursa Wolf transformation works on geocentric
	/// coordinates, so you cannot apply it onto geographic coordinates directly. If there are only three
	/// parameters then you can use the Molodenski or abridged Molodenski formulas.</para>
	/// <para>If a datums ToWgs84Parameters parameter values are zero, then the receiving
	/// application can assume that the writing application believed that the datum is approximately equal to
	/// WGS84.</para>
	/// </remarks>
  public class BursaWolfGeographicTransform : DatumTransform
  {

    //const double DEG2RAD = 57.2957795130823;
    //private const double DEG2RAD = 180 / Math.PI;

    //private const double SEC_TO_RAD = 4.8481368110953599358991410235795e-6;
    //private const double SEC_TO_RAD = 1 / 3600 / DEG2RAD;

    //local copy of spheroid parameters
    private double a;   // source spheroid semi major axis 
    private double e2;  // source spheroid , fisrt eccentricity
    private double A;   // target spheroid semi major axis
    private double E2;  // target spheroid , first eccentricity
    private double E22; // target spheroid , second eccentricity

    // transform variables
    /// <summary>
    /// Bursa Wolf shift vector in meters.
    /// </summary>
    private Vector pos;

    /// <summary>
    /// Bursa Wolf rotation in arc seconds.
    /// </summary>
    private Vector rot;

    /// <summary>
    /// Bursa Wolf scaling in parts per million.
    /// </summary>
    private double m;

    /// <summary>
    /// default height of location where
    /// transform should be processed
    /// above spheroid surface in this area
    /// not mandatory, in not supplied 0 is applied (spheroid level)
    /// </summary>
    private double h = 0;

    private Coordinate in3D;
    private Coordinate out3D;
    private Coordinate outP;
    private double meridianDifference;
    private double N, W;


    #region constructors

    /// <summary>
    /// xaml constr.
    /// </summary>
    public BursaWolfGeographicTransform() 
      :base()
    {
      Metadata = new Metadata("Bursa-Wolf 3d datum transform", "none", -1, "", "", "provides transformation between different datums based on parameter matrix");
    }





    /// <summary>
		/// Initializes an instance of Wgs84ConversionInfo
		/// </summary>
		/// <param name="dx">Bursa Wolf shift in meters.</param>
		/// <param name="dy">Bursa Wolf shift in meters.</param>
		/// <param name="dz">Bursa Wolf shift in meters.</param>
		/// <param name="ex">Bursa Wolf rotation in arc seconds.</param>
		/// <param name="ey">Bursa Wolf rotation in arc seconds.</param>
		/// <param name="ez">Bursa Wolf rotation in arc seconds.</param>
		/// <param name="ppm">Bursa Wolf scaling in parts per million.</param>
		public BursaWolfGeographicTransform(GeographicCoordinateSystem sourceCS, 
                              GeographicCoordinateSystem targetCS,
                              DistanceUnit distanceParamUnit,
                              AngularUnit angularParamUnit,
                              double dx, double dy, double dz,
                              double ex, double ey, double ez, double ppm)
			:this(sourceCS, targetCS,
        distanceParamUnit,
        angularParamUnit,
        false,
        new XParam("TranslationX", dx),
        new XParam("TranslationY", dy),
        new XParam("TranslationZ", dz),
        new XParam("RotationX", ex),
        new XParam("RotationY", ey),
        new XParam("RotationZ", ez),
        new XParam("ScaleFactor", ppm)
      )
		{  }





    public BursaWolfGeographicTransform(GeographicCoordinateSystem sourceCS,
                          GeographicCoordinateSystem targetCS,
                          DistanceUnit distanceParamUnit,
                          AngularUnit angularParamUnit,
                          bool createInverse, params XParam[] parameters
                          )
      : this(sourceCS, targetCS, distanceParamUnit, angularParamUnit, createInverse, new TransformParameters(parameters) )
    {  }





    public BursaWolfGeographicTransform(GeographicCoordinateSystem sourceCS,
                          GeographicCoordinateSystem targetCS,
                          DistanceUnit distanceParamUnit,
                          AngularUnit angularParamUnit,
                          bool createInverse, TransformParameters parameters
                          )
      : base(sourceCS, targetCS, distanceParamUnit, angularParamUnit, createInverse, parameters)
    {
      Metadata = new Metadata("Bursa-Wolf 3d datum transform", "none", -1, "", "", "provides transformation between different datums based on parameter matrix");
    }



    


    public BursaWolfGeographicTransform(GeographicCoordinateSystem sourceCS, 
                                        GeographicCoordinateSystem targetCS,
                                        DistanceUnit distanceParamUnit,
                                        AngularUnit angularParamUnit,
                                        TransformParameters parameters
                                        )
      : this(sourceCS, targetCS, distanceParamUnit, angularParamUnit, false, parameters)
    { }


    #endregion constructors





    protected override void updateTransform()
    {
      //if (_sourceCS == null || _targetCS == null || Parameters.Count != 7 || this.AngularParameterUnit == null || this.DistanceParameterUnit == null)
      //  return;

      if (!(Source is GeographicCoordinateSystem && Target is GeographicCoordinateSystem))
        throw new ArgumentException("source and target cs for bursa-wolf must be geographics cs");

      // prepare spheroid parameters
      GeographicCoordinateSystem cs1, cs2;
      if (!IsInverse)
      {
        cs1 = (GeographicCoordinateSystem)Source;
        cs2 = (GeographicCoordinateSystem)Target;
      }
      else
      {
        cs1 = (GeographicCoordinateSystem)Target;
        cs2 = (GeographicCoordinateSystem)Source;
      }
      a = cs1.Datum.Ellipsoid.SemiMajorAxis;
      A = cs2.Datum.Ellipsoid.SemiMajorAxis;
      e2 = cs1.Datum.Ellipsoid.e2;
      E2 = cs2.Datum.Ellipsoid.e2;
      E22 = cs2.Datum.Ellipsoid.eII2;
      //srcUnit = (AngularUnit)cs1.Unit;
      //trgtUnit = (AngularUnit)cs2.Unit;

      // get transform parameters
      pos = new Vector();
      rot = new Vector();

      rot.I = Parameters["RotationX"];
      rot.J = Parameters["RotationY"];
      rot.K = Parameters["RotationZ"];

      pos.I = Parameters["TranslationX"];
      pos.J = Parameters["TranslationY"];
      pos.K = Parameters["TranslationZ"];

      m = Parameters["ScaleFactor"];

      if (Parameters.ContainsKey("DefaultHeight"))
        h = Parameters["DefaultHeight"];
      else
        h = 0;

      // compute transform constants
      // and valiables

      //rot must go to computation in seconds
      //rot.I = rot.I / 3600 / DEG2RAD;
      //rot.J = rot.J / 3600 / DEG2RAD;
      //rot.K = rot.K / 3600 / DEG2RAD;
      rot.I = rot.I * AngularParameterUnit.RadiansPerUnit;
      rot.J = rot.J * AngularParameterUnit.RadiansPerUnit;
      rot.K = rot.K * AngularParameterUnit.RadiansPerUnit;

      // convert distance units
      pos.I = pos.I * DistanceParameterUnit.MetersPerUnit;
      pos.J = pos.J * DistanceParameterUnit.MetersPerUnit;
      pos.K = pos.K * DistanceParameterUnit.MetersPerUnit;

      //for inverse must 
      //change sign for pos, rot and m sign
      if (IsInverse)
      {
        pos.I *= -1;
        pos.J *= -1;
        pos.K *= -1;

        rot.I *= -1;
        rot.J *= -1;
        rot.K *= -1;

        m *= -1;
      }

      //point in, out
      in3D = new Coordinate();
      out3D = new Coordinate();
      outP = new Coordinate();

      // prepare meridian difference
      // sourceCS.MeridianGreenwichShift - TargetCS.MeridianGreenwichShift
      meridianDifference = (cs1.PrimeMeridian.Longitude * cs1.PrimeMeridian.AngularUnit.RadiansPerUnit)
        - (cs2.PrimeMeridian.Longitude * cs2.PrimeMeridian.AngularUnit.RadiansPerUnit);

    }





    //private void transform(ICoordinate P, Dimension dataDim)
    protected override void transformN(ICoordinate P)
    {

      //// convert input lat, lon point to radians
      //P.lon *= srcUnit.RadiansPerUnit;
      //P.lat *= srcUnit.RadiansPerUnit;

      // process meridian shift
      // for forward here
      if(!IsInverse)
        P.lon += meridianDifference;

      W = Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(P.lat), 2));
      N = a / W;
      //h = 230;

      // lon,lat to x1,y1,z1
      in3D.X = (N + h) * Math.Cos(P.lat) * Math.Cos(P.lon);
      in3D.Y = (N + h) * Math.Cos(P.lat) * Math.Sin(P.lon);

      // if default z (height) exists then use it
      if (h != 0)
        in3D.Z = (N * (1 - e2) + h) * Math.Sin(P.lat);
      else // use z height from input point
        in3D.Z = (N * (1 - e2) + P.Z) * Math.Sin(P.lat);

      //in3D.Z = (N * (1 - e2) + h) * Math.Sin(P.X);

      // x1,y1,z1 to x2,y2,z2
      out3D.X = (1 + m) * (in3D.X + rot.K * in3D.Y - rot.J * in3D.Z) + pos.I;
      out3D.Y = (1 + m) * (-rot.K * in3D.X + in3D.Y + rot.I * in3D.Z) + pos.J;
      out3D.Z = (1 + m) * (rot.J * in3D.X - rot.I * in3D.Y + in3D.Z) + pos.K;

      // x2,y2,z2 to lon,lat
      outP.lon = Math.Atan(out3D.Y / out3D.X);
      outP.lat = Math.Atan(out3D.Z * (1 + E22) / Math.Sqrt((out3D.X * out3D.X) + (out3D.Y * out3D.Y)));

      W = Math.Sqrt(1 - E2 * Math.Pow(Math.Sin(outP.lat), 2));
      N = A / W;

      // iteration to get highest possible precision
      for (int t = 0; t <= 5; t++)
      {
        outP.lat = Math.Atan((out3D.Z + N * E2 * Math.Sin(outP.lat)) / Math.Sqrt((out3D.lat * out3D.lat) + (out3D.lon * out3D.lon)));
        W = Math.Sqrt(1 - E2 * Math.Pow(Math.Sin(outP.lat), 2));
        N = A / W;
      }

      // process meridian shift
      // for inverse here
      if (IsInverse)
        outP.lon += meridianDifference;

      //// copy to output
      //P.lon = outP.lon / trgtUnit.RadiansPerUnit;
      //P.lat = outP.lat / trgtUnit.RadiansPerUnit;

      // if input coordinate has z (height value) or default height exists
      // compute also output height
      if (P.Z != 0 || h != 0)
      {
        //prepare height...
        W = Math.Sqrt(1 - E2 * Math.Pow(Math.Sin(outP.lat), 2));
        N = A / W;

        in3D.X = (N + h) * Math.Cos(outP.lat) * Math.Cos(outP.lon);
        in3D.Y = (N + h) * Math.Cos(outP.lat) * Math.Sin(outP.lon);
        in3D.Z = (N * (1 - e2)) * Math.Sin(outP.lat);

        outP.Z = Math.Sqrt(Math.Pow(in3D.X - out3D.X, 2) + Math.Pow(in3D.Y - out3D.Y, 2) + Math.Pow(in3D.Z - out3D.Z, 2));

        P.Z = outP.Z;
      }

    }





    /// <summary>
    /// Affine Bursa-Wolf matrix transformation
    /// </summary>
    /// <remarks>
    /// <para>Transformation of coordinates from one geographic coordinate system into another 
    /// (also colloquially known as a "datum transformation") is usually carried out as an 
    /// implicit concatenation of three transformations:</para>
    /// <para>[geographical to geocentric >> geocentric to geocentric >> geocentric to geographic</para>
    /// <para>
    /// The middle part of the concatenated transformation, from geocentric to geocentric, is usually 
    /// described as a simplified 7-parameter Helmert transformation, expressed in matrix form with 7 
    /// parameters, in what is known as the "Bursa-Wolf" formula:<br/>
    /// <code>
    ///  S = 1 + Ppm/1000000
    ///  [ Xt ]    [     S   -Ez*S   +Ey*S   Dx ]  [ Xs ]
    ///  [ Yt ]  = [ +Ez*S       S   -Ex*S   Dy ]  [ Ys ]
    ///  [ Zt ]    [ -Ey*S   +Ex*S       S   Dz ]  [ Zs ]
    ///  [ 1  ]    [     0       0       0    1 ]  [ 1  ]
    /// </code><br/>
    /// The parameters are commonly referred to defining the transformation "from source coordinate system 
    /// to target coordinate system", whereby (XS, YS, ZS) are the coordinates of the point in the source 
    /// geocentric coordinate system and (XT, YT, ZT) are the coordinates of the point in the target 
    /// geocentric coordinate system. But that does not define the parameters uniquely; neither is the
    /// definition of the parameters implied in the formula, as is often believed. However, the 
    /// following definition, which is consistent with the "Position Vector Transformation" convention, 
    /// is common E&amp;P survey practice: 
    /// </para>	
    /// <para>(dX, dY, dZ): Translation vector, to be added to the point's position vector in the source 
    /// coordinate system in order to transform from source system to target system; also: the coordinates 
    /// of the origin of source coordinate system in the target coordinate system </para>
    /// <para>(RX, RY, RZ): Rotations to be applied to the point's vector. The Sign convention is such that 
    /// a positive rotation about an axis is defined as a clockwise rotation of the position vector when 
    /// viewed from the origin of the Cartesian coordinate system in the positive direction of that axis;
    /// e.g. a positive rotation about the Z-axis only from source system to target system will result in a
    /// larger longitude value for the point in the target system. Although rotation angles may be quoted in
    /// any angular unit of measure, the formula as given here requires the angles to be provided in radians.</para>
    /// <para>: The scale correction to be made to the position vector in the source coordinate system in order 
    /// to obtain the correct scale in the target coordinate system. M = (1 + dS*10-6), whereby dS is the scale
    /// correction expressed in parts per million.</para>
    /// <para><see href="http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs35.html"/> for an explanation of the Bursa-Wolf transformation</para>
    /// </remarks>
    /// <returns></returns>
    //private double[] GetAffineTransform()
    //{
    //  double RS = 1 + Ppm * 0.000001;
    //  return new double[7] { RS, Ex * SEC_TO_RAD * RS, Ey * SEC_TO_RAD * RS, Ez * SEC_TO_RAD * RS, Dx, Dy, Dz };
    //  /*return new double[3,4] {
    //    { RS,				-Ez*SEC_TO_RAD*RS,	+Ey*SEC_TO_RAD*RS,	Dx} ,
    //    { Ez*SEC_TO_RAD*RS,	RS,					-Ex*SEC_TO_RAD*RS,	Dy} ,
    //    { -Ey*SEC_TO_RAD*RS,Ex*SEC_TO_RAD*RS,	RS,					Dz}
    //  };*/
    //}


    protected override bool parametersValid()
    {
      return (Parameters.Count == 7);
    }




    public override SRSTransform Inverse()
    {
      return new BursaWolfGeographicTransform(
                      (GeographicCoordinateSystem)Source,
                      (GeographicCoordinateSystem)Target,
                      base.DistanceParameterUnit,
                      base.AngularParameterUnit,
                      !IsInverse,
                      Parameters
                      );
    }




    public override Dimension TransformDimension
    {
      get { return Dimension.dim3D; }
    }



  }
}
